#include <iostream>
#include <opencv4/opencv2/opencv.hpp>
#include <string>

using namespace std;
string image_file="./distort.png";

int main(int argc, char **argv) 
{
    double k1=-0.28340811,k2=0.7395907,p1=0.00019359,p2=1.76187114e-5;
    double fx=458.36,fy=457.296,cx=367.215,cy=248.375;
    
    cv::Mat image=cv::imread(image_file,0);
    int rows=image.rows,cols=image.cols;
    cv::Mat image_undistort=cv::Mat(rows,cols,CV_8UC1);
    
    for(int v=0;v<rwos;v++) //畸变过程
    {
        for(int u=0;u<cols;u++)
        {
            double x=(u-cx)/fx,y=(v-cy)/fy;
            double r=sqrt(x*x+y*y);
            double x_distorted=x*(1+k1*r*r+k2*r*r*r*r)+2*p1*x*y+p2*(r*r+2*x*x);
            double y_distorted=y*(1+k1*r*r+k2*r*r*r*r)+2*p2*x*y+p1*(r*r+2*y*y);
            double u_distorted=fx*x_distorted+cx;
            double v_distorted=fy*y_distorted+cy;
            
        //浮点变整形采用最近邻插值法；
            if(u_distorted>=0&&v_distorted>=0&&u_distorted<cols&&v_distorted<rows)
            {
                image_undistort.at<uchar>(v,u)=image.at<uchar>((int)v_distorted,(int)u_distorted);
            }
            else
            {
                image_undistort.at<uchar>(v,u)=0;
            }
        }
    }
        
    cv::imshow("distorted",image);
    cv::imshow("undistorted",image_undistort);  //命名错误，应该是畸变后的图像；
    cv::waitKey(0);
    return 0;        
}
